//
// Created by ZhaoXiaoFei on 2022/8/19.
//

#include "model/gpsCorrect.hpp"

GpsCorrect::GpsCorrect() {

}

void GpsCorrect::correct(const GpsPositionData& GpsData, State* state){
    //
    Eigen::Vector3d utmPoint;
    GeographicLib::LocalCartesian local_cartesian;
    local_cartesian.Reset(state->initLLA(0), state->initLLA(1), state->initLLA(2));
    local_cartesian.Forward(GpsData.lla(0), GpsData.lla(1), GpsData.lla(2),
                            utmPoint.data()[0], utmPoint.data()[1], utmPoint.data()[2]);

    Eigen::Vector3d Y = utmPoint - state->pose_.block<3, 1>(0,3);
    Eigen::Matrix<double, 3, 15> H = Eigen::Matrix<double, 3, 15>::Zero();
    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();

    Eigen::Matrix3d V;
    V << 1.5,0,0,0,1.5,0,0,0,4.0;

    State* lastState = state;
    Eigen::MatrixXd K = lastState->P_ * H.transpose() * (H * lastState->P_ * H.transpose() + V).inverse();
    state->P_ = (Eigen::Matrix<double, 15, 15>::Identity() - K * H) * lastState->P_;
    state->X_ = lastState->X_ + K * (Y - H * lastState->X_);

    //inject error state
    state->pose_.block<3, 1>(0, 3) = lastState->pose_.block<3, 1>(0, 3)  + state->X_.segment<3>(0);
    state->vel_ = lastState->vel_ + state->X_.segment<3>(3);
    const Eigen::Vector3d del_theta = state->X_.segment<3>(6);
    if (del_theta.norm() >= 1e-12){
        state->pose_.block<3,3>(0,0) = lastState->pose_.block<3,3>(0,0)  * Eigen::AngleAxisd(del_theta.norm(),del_theta.normalized()).toRotationMatrix();
    }
    state->acc_bias_ = lastState->acc_bias_ + state->X_.segment<3>(9);
    state->gyr_bias_ = lastState->gyr_bias_ + state->X_.segment<3>(12);

}